#include "Planner.h"
#include "EditDistanceCalculator.hpp"

#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>

using namespace std;

#define CLEARANCE_EPSILON 1

const UINT32 Planner::INITIAL_NUMBER_OF_VERTICES=1;

Planner::Planner(Scene* scene, int time, boolean measure, double alpha, vector<vector<Conf>>* path, double* quality): m_scene(scene), _timer(time*1000), _use_clearance(measure),_alpha(alpha),
	_path_return(path), _quality_return(quality)
{
	extract_scenario_from_gui();
	if(_use_clearance)
	{
		vector<Polygon_2>* obstacle_vector=new vector<Polygon_2>();
		for (std::list<Polygon_2>::iterator _it = m_obstacles.begin();
			_it != m_obstacles.end();_it++)
		{
			obstacle_vector->push_back(*_it);
		}
		r1_clearance_detector=new Clearance_detector_ie(obstacle_vector,m_robot_polygons[0],CLEARANCE_EPSILON);
		r2_clearance_detector=new Clearance_detector_ie(obstacle_vector,m_robot_polygons[1],CLEARANCE_EPSILON);
	}
}

Planner::~Planner()
{}

Prm Planner::get_prm_with_solution(UINT32& number_of_vertices_to_try,CollisionDetector* collision_detector,MultiRobotsSampler* multi_robots_sampler)
{
	Prm roadmap(number_of_vertices_to_try,
		12,
		collision_detector,
		multi_robots_sampler,
		m_start_confs, m_target_confs,!_use_clearance);
	if(_use_clearance)
	{
		roadmap.set_clearance_detector(r1_clearance_detector,r2_clearance_detector,_alpha);
	}
	roadmap.generate_roadmap();
	while(true)
	{
		printf("Trying to get solution with: %d\n",number_of_vertices_to_try);
		roadmap.generate_path();
		if(roadmap.has_path())
		{
			printf("found solution, quality is: %f\n",roadmap.get_path_quality());
			return roadmap;
		}
		roadmap.add_vertices(number_of_vertices_to_try);
		number_of_vertices_to_try=number_of_vertices_to_try*2;
	}
}

Prm Planner::merge_two_solutions(Prm& prm1,Prm& prm2)
{
	vector<ConfSet> primary_path;
	vector<ConfSet>	secondary_path;
	primary_path=prm1.retrieve_path();
	secondary_path=prm2.retrieve_path();
	printf("merging paths of quality %f and %f\n",prm1.get_path_quality(),prm2.get_path_quality());
	EditDistanceCalculator calc(primary_path,secondary_path);
	std::vector<std::vector<UINT32>> connections_to_try=calc.get_connections_to_try();

	std::vector<std::vector<ConfSet>> points_to_connect;
	for(UINT32 i=0;i<connections_to_try.size();i++)
	{
		std::vector<ConfSet> pair_to_try;
		UINT32 primary_index=connections_to_try[i][0];
		UINT32 secondary_index=connections_to_try[i][1];
		if(primary_index<=primary_path.size() && secondary_index<=secondary_path.size())
		{
			pair_to_try.push_back(primary_path[primary_index]);
			pair_to_try.push_back(secondary_path[secondary_index]);
			points_to_connect.push_back(pair_to_try);
		}		
	}
	prm1.add_path(secondary_path);
	prm1.add_connections(points_to_connect); 
	prm1.generate_path();
	printf("new quality is %f\n",prm1.get_path_quality());
	return prm1;
}

Prm Planner::get_combined_solution(UINT32 number_of_solutions,UINT32 number_of_vertices_to_start_from,CollisionDetector* collision_detector,MultiRobotsSampler* multi_robots_sampler)
{
	if(_timer.is_time_up())
		return Prm();
	if(number_of_solutions==1)
	{
		return get_prm_with_solution(number_of_vertices_to_start_from,collision_detector,multi_robots_sampler);
	}
	Prm prm1=get_combined_solution(number_of_solutions/2,number_of_vertices_to_start_from,collision_detector,multi_robots_sampler);
	if(prm1.is_null())
		return Prm();
	Prm prm2=get_combined_solution(number_of_solutions-number_of_solutions/2,number_of_vertices_to_start_from,collision_detector,multi_robots_sampler);
	if(prm2.is_null())
		return prm1;
	return merge_two_solutions(prm1,prm2);
}



/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run()
{
	CollisionDetector* collision_detector = new CollisionDetector(m_robot_polygons, &m_obstacles);
	MultiRobotsSampler* multi_robots_sampler = new MultiRobotsSampler(m_robot_polygons, m_room,collision_detector,&m_obstacles,!_use_clearance);
	UINT32 successful_amount_of_vertices=INITIAL_NUMBER_OF_VERTICES;
	Prm main_prm=get_prm_with_solution(successful_amount_of_vertices,collision_detector,multi_robots_sampler);
	
	UINT32 number_of_paths_to_merge=_timer.fraction_of_time_passed()/2;
//	UINT32 number_of_paths_to_merge=1;
	if(number_of_paths_to_merge>0)
	{
		printf("#########Going to merge %d paths\n",number_of_paths_to_merge);
		Prm secondary_prm=get_combined_solution(number_of_paths_to_merge,successful_amount_of_vertices,collision_detector,multi_robots_sampler);
		if(!secondary_prm.is_null())
			merge_two_solutions(main_prm,secondary_prm);
	}	
	m_path=main_prm.retrieve_path();
	if(_path_return!=NULL)
	{
		*_path_return=m_path;
	}
	if(_quality_return!=NULL)
	{
		*_quality_return=main_prm.get_path_quality();
	}
/*
	Prm roadmap1(800,
				12,
				collision_detector,
				multi_robots_sampler,
				m_start_confs, m_target_confs);

	roadmap1.generate_roadmap();

	Prm roadmap2(800,
		12,
		collision_detector,
		multi_robots_sampler,
		m_start_confs, m_target_confs);

	roadmap2.generate_roadmap();

	vector<ConfSet>	m_path1;
	vector<ConfSet>	m_path2;

	roadmap1.retrieve_path(&m_path1);
	roadmap2.retrieve_path(&m_path2);
	EditDistanceCalculator calc(m_path1,m_path2);
	std::vector<std::vector<UINT32>> connections_to_try=calc.get_connections_to_try();

	std::vector<std::vector<ConfSet>> points_to_connect;
	for(UINT32 i=0;i<connections_to_try.size();i++)
	{
		std::vector<ConfSet> pair_to_try;
		pair_to_try.push_back(m_path1[connections_to_try[i][0]]);
		pair_to_try.push_back(m_path2[connections_to_try[i][1]]);
		points_to_connect.push_back(pair_to_try);
	}

	roadmap1.add_path(m_path2);
	roadmap1.add_connections(points_to_connect);
	roadmap1.retrieve_path(&m_path);

	*/
	transform_path_for_gui();
}


Point_2	Planner::transformToCGAL(QPointF qp)
{
	// x and y coordinates of the point
	double x = qp.x();
	double y = qp.y();

	Point_2 cpoint(x,y);
	return cpoint;
}
QPointF	Planner::transformToQT(Conf cp)
{
	return QPointF(CGAL::to_double(cp.x()) , CGAL::to_double(cp.y()) );
}

QVector<QPointF> Planner::transformToQT(ConfSet cs)
{
	QVector<QPointF> result;

	foreach(Conf c, cs)
	{
		result.push_back(transformToQT(c));
	}
	return result;
}

QVector<QVector<QPointF> > Planner::transformToQT(list<vector<Conf> > path)
{
	QVector<QVector<QPointF> > result;

	foreach(ConfSet cs, path)
	{
		result.push_back(transformToQT(cs));
	}
	return result;
}

void Planner::transform_path_for_gui()
{
	vector<vector<vector<QConf> > > result_vectors(m_path.size());

	for (int i = 0; i < m_path.size(); i++)
	{
		for (int r = 0; r < m_robot_num; r++)
		{
			vector<QConf> path_for_r;

			QPointF vec;
			double angle = 0;
			if (i != 0)
			{
				QPointF s = transformToQT(m_path[i-1][r]);
				QPointF t = transformToQT(m_path[i][r]);
				vec = t - s;
			}
			else
				vec = transformToQT(m_path[i][r]);

			QConf qconf(vec,angle);
			path_for_r.push_back(qconf);

			result_vectors[i].push_back(path_for_r);
		}
	}

	/*	If "result" is empty, the GUI assumes that a solution haven't been found */
	m_scene->setPath(result_vectors);	
}

void Planner::extract_scenario_from_gui()
{
	Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
	Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
	Point_2 pbl(ptl.x(), pbr.y());
	Point_2 ptr(pbr.x(), ptl.y());
	m_room = Room(ptl,pbr);

	QVector<QPolygonF> obstacles = m_scene->getObstacles();

	vector<QPolygonF> robots = m_scene->getRobotPolygons();
	foreach(QPolygonF qp, robots)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_robot_polygons.push_back(cp);
	}

	foreach(QPolygonF qp, obstacles)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_obstacles.push_back(cp);
	}

	Polygon_2 wall_left;
	wall_left.push_back(pbl);
	wall_left.push_back(ptl);

	Polygon_2 wall_top;
	wall_top.push_back(ptl);
	wall_top.push_back(ptr);

	Polygon_2 wall_right;
	wall_right.push_back(ptr);
	wall_right.push_back(pbr);

	Polygon_2 wall_bot;
	wall_bot.push_back(pbl);
	wall_bot.push_back(pbr);

	m_obstacles.push_back(wall_left);
	m_obstacles.push_back(wall_top);
	m_obstacles.push_back(wall_right);
	m_obstacles.push_back(wall_bot);

	QVector<QVector<QPointF> > startPositions = m_scene->getStartPositions();
	QVector<QVector<QPointF> > targetPositions = m_scene->getTargetPositions();

	m_robot_num = targetPositions.size();

	for (int g = 0; g < m_robot_num; g++)
	{
		m_start_confs.push_back(transformToCGAL(startPositions[g][0]));
		m_target_confs.push_back(transformToCGAL(targetPositions[g][0]));
	}
}